#include "gnss_imu_align/gnss_imu_align_wrapper.h"
#include "nav_msgs/Path.h"
#include "nav_msgs/Odometry.h"
GNSS_IMU_Align_Wrapper::GNSS_IMU_Align_Wrapper(/* args */)
{
    ros::NodeHandle nh;
     
    nh.param<std::string>("gnss_topic",gnss_topic,"/usv_self");
    nh.param<std::string>("imu_topic",imu_topic,"/wit_imu");
    nh.param<std::string>("path_topic",path_topic,"/path");
    nh.param<std::string>("gnss_imu_odom_topic",odom_topic,"/gi_odometry");
    nh.param<int>("gnss_type",gnss_type,0);
    gnss_buffer.clear();
    imu_buffer.clear();
    if (gnss_type ==xt_usv){
        gnss_sub = nh.subscribe(gnss_topic,100,&GNSS_IMU_Align_Wrapper::xt_gnss_sub_cb,this);
        std::cout<<"gnss sensor type";
        std::cout<<" xt usv "<<std::endl;
    }else{
        gnss_sub = nh.subscribe(gnss_topic,100,&GNSS_IMU_Align_Wrapper::normal_gnss_sub_cb,this);
        std::cout<<"gnss sensor type";
        std::cout<<" normal gnss "<<std::endl;
    }
    imu_sub = nh.subscribe(imu_topic,100,&GNSS_IMU_Align_Wrapper::imu_sub_cb,this);
    path_pub = nh.advertise<nav_msgs::Path>(path_topic,100);
    odom_pub = nh.advertise<nav_msgs::Odometry>(odom_topic,100);
    run();
}

GNSS_IMU_Align_Wrapper::~GNSS_IMU_Align_Wrapper()
{
}
void GNSS_IMU_Align_Wrapper::xt_gnss_sub_cb(const self_localization_transform::usv_selfConstPtr &msg){
    static uint64_t skip_gnss_cnt = 0;
    SlamCraft::GNSSDataType receive_gnss_msg(msg->gps.latitude,msg->gps.longitude,msg->gps.altitude,msg->header.stamp.toSec());
    receive_gnss_msg.vel.x()= msg->v_enu.v_e;
    receive_gnss_msg.vel.y()= msg->v_enu.v_n;
    receive_gnss_msg.vel.z()= msg->v_enu.v_u;
    if (skip_gnss_cnt%2 ==0)
    {
        gnss_buffer.push_back(receive_gnss_msg);
    }
}
void GNSS_IMU_Align_Wrapper::normal_gnss_sub_cb(const sensor_msgs::NavSatFixConstPtr &msg){
    SlamCraft::GNSSDataType receive_gnss_msg(msg->latitude,msg->longitude,0.0,msg->header.stamp.toSec());
    auto&cov_matrix = msg->position_covariance;
    receive_gnss_msg.cov << cov_matrix[0],cov_matrix[1],cov_matrix[2],
                            cov_matrix[3],cov_matrix[4],cov_matrix[5],
                            cov_matrix[6],cov_matrix[7],cov_matrix[8];
    gnss_buffer.push_back(receive_gnss_msg);
    
}
void GNSS_IMU_Align_Wrapper::imu_sub_cb(const sensor_msgs::ImuConstPtr &msg){
    SlamCraft::IMU receive_imu_msg;
    receive_imu_msg.acc = {msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z};
    receive_imu_msg.gro = {msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z};
    receive_imu_msg.stamp = msg->header.stamp.toSec();
    receive_imu_msg.rot.w() = msg->orientation.w;
    receive_imu_msg.rot.x() = msg->orientation.x;
    receive_imu_msg.rot.y() = msg->orientation.y;
    receive_imu_msg.rot.z() = msg->orientation.z;
    imu_buffer.push_back(receive_imu_msg);
}
/// @brief 主优化步骤 1) 先进行数据同步; 2)未进行初始化，则进行初始化; 3) 进行eskf融合
void GNSS_IMU_Align_Wrapper::run(){
    
    ros::Rate rate(3000);
    MeasureGroupType measure_group;
    while (ros::ok())
    {
        rate.sleep();
        ros::spinOnce();
        measure_group.clear();
        if(!sync_measure_group(measure_group)){
            continue;   
        }
        if (!gnss_imu_align.init(measure_group))
        {
            continue;
        }
        gnss_imu_align.process_group(measure_group);
        publish_path(gnss_imu_align.get_x().position,measure_group.end_gnss.stamp);
        //publish_odometry(gnss_imu_align.get_x(),measure_group.end_gnss.stamp);
        Eigen::Vector3d pos;
        gnss_imu_align.get_geo_cvt().get_relative_position(measure_group.end_gnss,pos);
        publish_odometry(pos,measure_group.end_gnss.stamp);
    }

}
bool GNSS_IMU_Align_Wrapper::sync_measure_group(MeasureGroupType&measure_group){
    if (gnss_buffer.empty()||imu_buffer.empty())
    {
        return false;
    }
    while (!gnss_buffer.empty()&&gnss_buffer.front().stamp<imu_buffer.front().stamp)
    {
        gnss_buffer.pop_front();
    }
    if (gnss_buffer.empty())
    {
        return false;
    }
    
    if (gnss_buffer.front().stamp>imu_buffer.back().stamp)
    {
        return false;
    }
    measure_group.end_gnss = gnss_buffer.front();
    while (imu_buffer.front().stamp<measure_group.end_gnss.stamp)
    {
        measure_group.imus_in.push_back(imu_buffer.front());
        imu_buffer.pop_front();
    }
    gnss_buffer.pop_front();
    return true;
    
    
}
void GNSS_IMU_Align_Wrapper::publish_path(Eigen::Vector3d t,double time_stamp){
    static nav_msgs::Path path;
    path.header.frame_id = "map";
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = t.x();
    pose.pose.position.y = t.y();
    pose.pose.position.z = t.z();
    pose.header.stamp = ros::Time(time_stamp);
    path.poses.push_back(pose);
    path.header.stamp = ros::Time(time_stamp);
    path_pub.publish(path);
    
}
void GNSS_IMU_Align_Wrapper::publish_odometry( SlamCraft::ESKF::State_18 state_18,double time_stamp){
    nav_msgs::Odometry odom_message;
    odom_message.header.frame_id = "map";
    odom_message.header.stamp = ros::Time(time_stamp);
    odom_message.child_frame_id = "body";
    odom_message.pose.pose.position.x = state_18.position.x();
    odom_message.pose.pose.position.y = state_18.position.y();
    odom_message.pose.pose.position.z = state_18.position.z();
    odom_message.pose.pose.orientation.w = state_18.rotation.w();
    odom_message.pose.pose.orientation.x = state_18.rotation.x();
    odom_message.pose.pose.orientation.y = state_18.rotation.y();
    odom_message.pose.pose.orientation.z = state_18.rotation.z();
    odom_pub.publish(odom_message);
}
void GNSS_IMU_Align_Wrapper::publish_odometry( Eigen::Vector3d position,double time_stamp){
    nav_msgs::Odometry odom_message;
    odom_message.header.frame_id = "map";
    odom_message.header.stamp = ros::Time(time_stamp);
    odom_message.child_frame_id = "body";
    odom_message.pose.pose.position.x = position.x();
    odom_message.pose.pose.position.y = position.y();
    odom_message.pose.pose.position.z = position.z();

    odom_pub.publish(odom_message);
}